/*
 * Copyright (c) 2013 Johann Hanne
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "i2c.h"
#include "sensor.h"

#include <stdio.h>

static uint8_t sensor_i2c_addr;

void sensor_init(void) {
    uint16_t reg_ff;

    /* 0x5d: Address of MT9V011 (aka MI0360) */
    sensor_i2c_addr = 0x5d;
    if (i2c_write(sensor_i2c_addr, NULL, 0) == I2C_OK) {
        printf("Found image sensor at I2C address 0x5d\n");
    } else {
        /* 0x5c: Address of MT9V111 (aka K82) */
        sensor_i2c_addr = 0x5c;
        if (i2c_write(sensor_i2c_addr, NULL, 0) == I2C_OK) {
            printf("Found image sensor at I2C address 0x5c\n");
        } else {
            sensor_i2c_addr = 0x00;
            printf("Did not find an image sensor!\n");
            return;
        }
    }

    if (sensor_readreg(0xff, &reg_ff) != 0) {
        sensor_i2c_addr = 0x00;
        printf("Failed to read image sensor register 0xff!\n");
        return;
    }

    printf("Image sensor register 0xff value: 0x%04x\n", (unsigned int)reg_ff);
    
    if (sensor_i2c_addr == 0x5d && reg_ff == 0x8243) {
        printf("Identified image sensor MT9V011 (aka MI0360)\n");
    } else if (sensor_i2c_addr == 0x5c && reg_ff == 0x823a) {
        printf("Identified image sensor MT9V111 (aka K82)\n");
    } else {
        sensor_i2c_addr = 0x00;
        printf("Unknown image sensor!\n");
        return;
    }
}

void sensor_set_defaults(void) {
    sensor_writereg(0x0d, 0x0001);
    sensor_writereg(0x0d, 0x0000);

    if (sensor_i2c_addr == 0x5c) {
        sensor_writereg(0x01, 0x0001);
        sensor_writereg(0x06, 0x300c);
        sensor_writereg(0x08, 0xcc00);
        sensor_writereg(0x01, 0x0004);
    }

    sensor_writereg(0x30, 0x0005); /* Reserved */
    sensor_writereg(0x34, 0x0100); /* Reserved */
    sensor_writereg(0x3d, 0x068f); /* Reserved */
    sensor_writereg(0x40, 0x0520); /* Reserved */
    sensor_writereg(0x05, 0x007b); /* Horizontal blanking */
    sensor_writereg(0x06, 0x001c); /* Vertical blanking */
    sensor_writereg(0x03, 0x01e5); /* Window height */
    sensor_writereg(0x04, 0x0285); /* Window width */
    sensor_writereg(0x35, 0x01c0); /* Global gain */
}

int sensor_readreg(uint8_t reg, uint16_t *val) {
    uint8_t buf[2];
    if (i2c_write_restart_read(sensor_i2c_addr, &reg, 1,
            sensor_i2c_addr, buf, 2) != I2C_OK) {
        return -1;
    }
    *val = ((uint16_t)buf[0] << 8) | (uint16_t)buf[1];
    return 0;
}

int sensor_writereg(uint8_t reg, uint16_t val) {
    uint8_t buf[3];
    buf[0] = reg;
    buf[1] = (val & 0xff00) >> 8;
    buf[2] =  val & 0x00ff;
    if (i2c_write(sensor_i2c_addr, buf, 3) != I2C_OK) {
        return -1;
    }
    return 0;
}
